
#include"motion_controller/pid.hpp"
PidCompute::PidCompute(float kp_, float ki_, float kd_) : kp(kp_), ki(ki_), kd(kd_) {}
PidCompute::~PidCompute() {}
void PidCompute :: IntegralSaturation(float &input, float max)
{
  if (input > max) 
  {
    input = max;
  }
  if (input < -max) 
  {
    input = -max;
  }

}

// float PidCompute :: PidCtrl(float act, float tar)
// {
//   float pid_output;
//   actual = act;
//   target = tar;
//   error = target - actual;
//   integral += error;
//   IntegralSaturation(integral, 0.3);

//   pid_output = kp * error + ki * integral +kd * (error-error_last);
//   error_last = error;

//   return pid_output;
// }

float PidCompute :: PidCtrl(float act, float tar)
{
  float pid_output;
  actual = act;
  target = tar;
  error = target - actual;

  integral = error + error_last + error_last_last + error_last_last_last + error_last_last_last_last;
  IntegralSaturation(integral, 0.5);

  pid_output = kp * error + ki * integral +kd * (error - error_last);

  error_last_last_last_last = error_last_last_last;
  error_last_last_last = error_last_last;
  error_last_last = error_last;
  error_last = error;

  return pid_output;
}



float PidCompute::incrementalPidCtrl(float act, float tar)
{
  float delta_pid_output;
  actual = act;
  target = tar;
  error = target - actual;
  delta_pid_output = kp * (error-error_last) + ki * error + kd * (error - 2 * error_last + error_last_last);
  
  error_last_last = error_last;
  error_last = error;
 
  return delta_pid_output;

}